#include <gtest/gtest.h>
#include <memory>
#include <Eigen/Core>
#include <ceres/ceres.h>
#include "ceres/gradient_checker.h"

#include "../include/PBAFactors.h"
#include "../include/PBABlocks.hpp"
#include "../../util/generateRandom.h"

using namespace Estimator;
using namespace ceres;

TEST(GRADIENT_TEST /*test_case_name*/, Jacobian_POSE1 /*test_name*/)
{
    Eigen::Vector3d measurement = Eigen::Vector3d::Random();
    Eigen::Matrix3d information = Eigen::Matrix3d::Identity();
    CostFunction* my_cost_function = new Estimator::CeresPBAFactor3Nodes(measurement, information);
    LocalParameterization* my_parameterization = new Estimator::PoseLocalParameterization();
    LocalParameterization* my_parameterization1 = new Estimator::PoseLocalParameterization();
    LocalParameterization* my_parameterization2 = new Estimator::LandMarkLocalParameterization();
    std::vector<const LocalParameterization*> local_parameterizations;
    local_parameterizations.push_back(my_parameterization);
    local_parameterizations.push_back(my_parameterization1);
    local_parameterizations.push_back(my_parameterization2);

    Eigen::Matrix<double, 3, 4, RowMajor> _transform1;
    _transform1.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform1.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose1(0, _transform1);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian1 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 3, 4, RowMajor> _transform2;
    _transform2.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform2.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose2(1, _transform2);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian2 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 5,1 > _landmark;
    _landmark.segment<2>(0) = Eigen::Vector2d::Random().normalized();
    _landmark.segment<3>(2) = Eigen::Vector3d::Random().normalized();
    LandMarkBlock landmark(1, _landmark);
    Eigen::Matrix<double, 3, 5, RowMajor> _jacobian3 = Eigen::Matrix<double, 3, 5>::Constant(std::nan(""));

    std::vector<const double*> parameter_blocks;
    parameter_blocks.push_back(pose1.parameters());
    parameter_blocks.push_back(pose2.parameters());
    parameter_blocks.push_back(landmark.parameters());

    std::vector<double* > jacobians(3);
    jacobians[0] = _jacobian1.data();
    jacobians[1] = _jacobian2.data();
    jacobians[2] = _jacobian3.data();
    Eigen::Vector3d residual;
    my_cost_function->Evaluate(parameter_blocks.data(), residual.data(), jacobians.data());

    NumericDiffOptions numeric_diff_options;
    GradientChecker gradientchecker(my_cost_function, &local_parameterizations, numeric_diff_options);

    GradientChecker::ProbeResults results;
    ////EXPECT_TRUE(gradientchecker.Probe(parameter_blocks.data(), 1e-9, &results));
    // std::cout << _jacobian1 << std::endl;
    // std::cout << "============"<< std::endl;

    // EXPECT_TRUE(gradientchecker.Probe(parameter_blocks.data(), 1e-9, &results)) << results.error_log;

    //ASSERT_EQ(results.return_value, true);

}

